#include "camera_adapter.h"
#include "bevdet_tensortrt/bevdet_data.h"
#include "camera_perception.h"
#include "muduo/base/CountDownLatch.h"
#include "muduo/base/Timestamp.h"
#include <utils/topic.h>
namespace camera_perception {

static std::map<std::string, bool> getCameras(void)
{
    static std::map<std::string, bool> cameras {
        { "camera0", true },
        { "camera1", true },
        { "camera2", true },
        { "camera3", true },
        { "camera4", true },
        { "camera5", true },
        { "camera6", true },
    };
    return cameras;
}

CameraAdapter::CameraAdapter(std::string name)
    : Node(std::move(name))
    , logger_(rclcpp::get_logger("CameraAdapter"))
{
    using namespace std::placeholders;
    cameraPerceptionSptr_ = std::make_shared<CameraPerception>();
    cameraPerceptionSptr_->setBevDetCallback(std::bind(&CameraAdapter::bevdetOneObjects, this, _1));
    cameraPerceptionSptr_->setBevDetMockCallback(std::bind(&CameraAdapter::bevdetMockOneObjects, this, _1));
    imageSReceiverSptr_ = this->create_subscription<interface::msg::Images>(imagesTopic, 30, std::bind(&CameraAdapter::onImages, this, _1));
    bevDetPubPtr_ = this->create_publisher<interface::msg::Objects>(cameraFusedObjectsTopic, 30);
}

void CameraAdapter::onImages(std::shared_ptr<interface::msg::Images> msg)
{
    for (auto& i : msg->images) {
        if (getCameras().find(i.header.frame_id) == getCameras().end()) {
            RCLCPP_ERROR(logger_, "Unsupported camera:%s", i.header.frame_id.c_str());
            continue;
        }
        RCLCPP_INFO(logger_, " camera:%s", i.header.frame_id);
    }
    std::cout << " onImage " << std::endl;
    cameraPerceptionSptr_->setImages(msg);
}

void CameraAdapter::bevdetOneObjects(const std::vector<Box> boxs)
{
    interface::msg::Objects objes;
    auto now = muduo::Timestamp::now();
    objes.header.stamp.sec = now.secondsSinceEpoch();
    objes.header.stamp.nanosec = (now.microSecondsSinceEpoch() % 1000000) * 1000;
    for (auto& i : boxs) {
        interface::msg::Object obj;
        obj.header.stamp.sec = objes.header.stamp.sec;
        obj.header.stamp.nanosec = objes.header.stamp.nanosec;
        obj.set__source(1); // 相机
        obj.set__id(0);
        BevdetObjectType type = BevdetObjectType(i.label);
        auto it = objectTypeMapBevdetType.find(type);
        if (it != objectTypeMapBevdetType.end()) {
            obj.set__type(int(it->second));
        } else {
            obj.set__type(0);
        }
        // obj.set__type();
        obj.set__x(i.x);
        obj.set__y(i.y);
        obj.set__z(i.z);
        obj.set__heading(0.0);
        obj.set__speed(0.0);
        obj.set__length(i.l);
        obj.set__width(i.w);
        obj.set__height(i.h);
        objes.objects.emplace_back(obj);
    }
    std::cout << " objes " << objes.objects.size() << std::endl;
    bevDetPubPtr_->publish(objes);
}

void CameraAdapter::bevdetMockOneObjects(const std::vector<Box> boxs)
{
    interface::msg::Objects objes;
    auto now = muduo::Timestamp::now();
    objes.header.stamp.sec = now.secondsSinceEpoch();
    objes.header.stamp.nanosec = (now.microSecondsSinceEpoch() % 1000000) * 1000;
    for (auto& i : boxs) {
        interface::msg::Object obj;
        obj.header.stamp.sec = objes.header.stamp.sec;
        obj.header.stamp.nanosec = objes.header.stamp.nanosec;
        obj.header.frame_id = "camera1";
        obj.set__source(1); // 相机
        obj.set__id(0);
        BevdetObjectType type = BevdetObjectType(i.label);
        auto it = objectTypeMapBevdetType.find(type);
        if (it != objectTypeMapBevdetType.end()) {
            obj.set__type(int(it->second));
        } else {
            obj.set__type(0);
        }
        // obj.set__type();
        obj.set__x(i.x);
        obj.set__y(i.y);
        obj.set__z(i.z);
        obj.set__heading(0.0);
        obj.set__speed(0.0);
        obj.set__length(i.l);
        obj.set__width(i.w);
        obj.set__height(i.h);
        objes.objects.emplace_back(obj);
    }
    for (auto& i : boxs) {
        interface::msg::Object obj;
        obj.header.stamp.sec = objes.header.stamp.sec;
        obj.header.stamp.nanosec = objes.header.stamp.nanosec;
        obj.header.frame_id = "camera2";
        obj.set__source(1); // 相机
        obj.set__id(0);
        BevdetObjectType type = BevdetObjectType(i.label);
        auto it = objectTypeMapBevdetType.find(type);
        if (it != objectTypeMapBevdetType.end()) {
            obj.set__type(int(it->second));
        } else {
            obj.set__type(0);
        }
        // obj.set__type();
        obj.set__x(i.x);
        obj.set__y(i.y);
        obj.set__z(i.z);
        obj.set__heading(0.0);
        obj.set__speed(0.0);
        obj.set__length(i.l);
        obj.set__width(i.w);
        obj.set__height(i.h);
        objes.objects.emplace_back(obj);
    }
    std::cout << " objes " << objes.objects.size() << std::endl;
    bevDetPubPtr_->publish(objes);
}

}